package robot;
import lejos.navigation.CompassNavigator;
import lejos.navigation.CompassPilot;
import lejos.navigation.Navigator;
import lejos.navigation.Pilot;
import lejos.nxt.*;
import lejos.nxt.addon.CompassSensor;

public class Config {
	
	public static final float wheelDiameter = 5.6f;
	public static final float trackWidth    = 11.5f;
	
	public static Motor      lMotor = Motor.B;
	public static Motor      rMotor = Motor.C;
	public static SensorPort usPort = SensorPort.S1;
	public static SensorPort liPort = SensorPort.S3;
	public static SensorPort csPort = SensorPort.S2;
	
	public static UltrasonicSensor usSensor = null;
//	public static LightSensor      liSensor = new LightSensor(liPort);
	public static CompassSensor    csSensor = null;
	
	public static CompassPilot pilot = null;
	public static CompassNavigator nav = null;
	
	
	public static void configurePorts() {
		usSensor = new UltrasonicSensor(usPort);
		csSensor = new CompassSensor(csPort);
//		usPort.setTypeAndMode(SensorConstants.TYPE_LOWSPEED, SensorConstants.MODE_RAW);
		//liPort.setTypeAndMode(SensorConstants.TYPE_LIGHT_ACTIVE, SensorConstants.MODE_PCTFULLSCALE);
		pilot = new ImprovedCompassPilot(csSensor, wheelDiameter, trackWidth, lMotor, rMotor);
		nav = new CompassNavigator(pilot);
		nav.setSpeed(200);
	}
}
